#include "Graph.h"
#include "Vectores.h"

#include <vector>
#include <algorithm>
#include <iostream>

using namespace std;

Node::Node(float x, float y){
    this->id = 0;
    this->position[0] = x;
    this->position[1] = y;
    costSoFar = 0;
}

float Node::getCost(){
    return costSoFar;
}

int Node::getId(){
    return this->id;
}

void Node::setPosition(float x, float y){
    this->position[0] = x;
    this->position[1] = y;
}

vector<Node *> Node::getNeighbors(){
    return this->neighbors;
}

const float* Node::getPosition(){
    return this->position;
}

float Node::operator[](int i) const{
    return this->position[i];
}

void Node::addOlor(Olor* olor){
  olores.push_back(olor);
}

// = = = = = = = = = = = = = = = = = = = = = = = = = = = =
Graph::Graph(){ }

const Node *Graph::getNode(int i){
return nodes[i];
}

vector<Node *> Graph::getNodes(){
return nodes;
}

void Graph::addNode(Node *a){
nodes.push_back(a);
a->id = nodes.size()-1;
}

void Graph::addEdge(Node *a, Node *b){
a->neighbors.push_back(b);
}

void Graph::addEdge(int a, int b){
nodes[a]->neighbors.push_back(nodes[b]);
}

void Graph::addDoubleEdge(Node *a, Node *b){
this->addEdge(a, b);
this->addEdge(b, a);
}

void Graph::addDoubleEdge(int a, int b){
this->addEdge(a, b);
this->addEdge(b, a);
}

bool IsOdd (int i) { return ((i%2)==1); }

vector<Node *> Graph::dijkstra(Node *start, Node *end){
int i, n, j, n2;
vector<Node *>resp;
vector<Node *>open;
vector<Node *>close;
vector<Node *>aux;
Node *current = NULL, *endNode;
float endNodeCost;
float vaux[3] = {0};
// Inicializamos el estado de todos los nodos
for(i=0, n=nodes.size(); i<n; i++){
this->_dijkstra_inicializeNode(nodes[i]);
}
// Inicializamos las listas
open.push_back(start);
while(open.size() > 0){
// Buscamos el elemento mas pequeno
current = open[0];
for(i=1, n=open.size(); i<n; i++){
  if(open[i]->costSoFar < current->costSoFar)
    current = open[i];
}
if(current->id == end->id)
  break;
for(j=0, n2=current->neighbors.size(); j<n2; j++){
    endNode = current->neighbors[j];
    restarVectores(current->getPosition(), endNode->getPosition(), vaux);
    endNodeCost = current->costSoFar + length(vaux);
    // Buscamos se esta en la lista de cerrados
    if(_dijkstra_inList(&close, endNode)){
        continue;
    } else if(_dijkstra_inList(&open, endNode)){
        if(endNode->costSoFar <= endNodeCost) continue;
    } else {
        // Es un nodo no visitado
    }
    // Actualizamos la informacion necesaria
    endNode->costSoFar = endNodeCost;
    endNode->connection = current;
    // La agregamos a la lista de abiertos
    if( !_dijkstra_inList(&open, endNode)){
        open.push_back(endNode);
    }
}
// Cerramos el nodo
aux.clear();
for(j=0, n2=open.size(); j<n2; j++){
    if(open[j]->id != current->id) aux.push_back(open[j]);
}
open = aux;
close.push_back(current);
}
if(!current) return resp;
if(current->id != end->id){
  return resp;
} else {
  while(current->id != start->id){
      resp.push_back(current->connection);
      current = current->connection;
  }
  std::reverse(resp.begin(), resp.end());
  resp.push_back(end);
}
return resp;
}

void Graph::_dijkstra_inicializeNode(Node *n){
n->costSoFar = 0;
n->connection = NULL;
}

bool Graph::_dijkstra_inList(vector<Node *>*list, Node *node){
int i, n;
for(i=0, n=list->size(); i<n; i++)
  if( (*list)[i]->id == node->id) return true;
return false;
}

float costNodePath(vector<Node *>path){
int i, n;
float cost, vaux[3];
for(i=0, n=path.size(), cost=0.0f; i<n-1; i++){
restarVectores(path[i]->getPosition(), path[i+1]->getPosition(), vaux);
cost = length(vaux);
}
return cost;
}

Node *Graph::findNearestNode(float point[2]){
    int i, n;
    float min, len;
    float aux[3] = {0};
    Node *min_node;
    Node *node = NULL;

    if(nodes.size() == 0) return NULL;

    min_node = nodes[0];
    restarVectores(nodes[0]->getPosition(), point, aux);
    min = length(aux);
    for(i=1, n=nodes.size(); i<n; i++){
        node = nodes[i];

        restarVectores(node->getPosition(), point, aux);
        len = length(aux);

        if(len < min){
            min = len;
            min_node = node;
        }
    }

    return min_node;
}
